#include "scan_process.hpp"
#include <cmath>
#include "file_sl.hpp"
//using namespace std;
void Scan_cloud::set_scan_input(sensor_msgs::msg::LaserScan::SharedPtr scan)
{
    std::size_t size = scan->ranges.size();
    struct scan_data sc_data;
    receive_scan.clear();
    for(int i=0;i<size;i++)
    {
       sc_data.angle= scan->angle_min + i * (scan->angle_increment) ;
       sc_data.distance = scan->ranges[i]; 
       if((sc_data.distance>-40.0)&&(sc_data.distance<40.0))
       receive_scan.push_back(sc_data);
    }
}
void Scan_cloud::convert_to_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_)
{
    std::size_t size;
    
     pcl::PointXYZ point_temp;
    size=receive_scan.size();
    cloud_->points.clear();
    for(int i=0;i<size;i++)
    {
        if(receive_scan[i].distance<40.0)
        {
            point_temp.x=receive_scan[i].distance*cos(receive_scan[i].angle);
            point_temp.y=receive_scan[i].distance*sin(receive_scan[i].angle);
            point_temp.z=0;
            cloud_->push_back(point_temp);
        }
        // if(i<10)std::cout<<i<<"="<<point_temp.x<<"  ";
    }
   // std::cout<<"sum= "<<size<<std::endl;
}
#define STEP_ANGLE 8
#define angle_cha 0.0
void Scan_cloud::find_angle_point(void)
{
    std::size_t size;
  //  std::vector<int>* index_;
    int i;
    std::vector<double> kk;
    int flaga,flagb,flagc;
   //  pcl::PointXYZ point_temp;
    size=receive_scan.size();
    //angle_cloud->clear();
    angle_index.clear();
    kk.resize(STEP_ANGLE);
    if(size>100)  //找角点
    {
       for( i=10;i<size-10;i++)
       {
            flaga=0;
            for(int j=0;j<STEP_ANGLE;j++)
            {
                kk[j]=receive_scan[i-1-j].distance-receive_scan[i-2-j].distance;
                if(kk[j]>-angle_cha)flaga++;
                if(kk[j]<angle_cha)flaga--;
            }
            flagb=0;
            for(int j=0;j<STEP_ANGLE;j++)
                {
                kk[j]=receive_scan[i+j+2].distance-receive_scan[i+j+1].distance;
                if(kk[j]>-angle_cha)flagb++;
                if(kk[j]<angle_cha)flagb--;
                } 
        flagc=0;
        if((flaga==STEP_ANGLE)&&(flagb==-STEP_ANGLE))flagc=-1;
        if((flaga==-STEP_ANGLE)&&(flagb==STEP_ANGLE))flagc=1;
        if(flagc!=0)
            {
                Angle_Index_struct ais;
                ais.index=i;
                ais.angle=receive_scan[i].angle;
                ais.distance =receive_scan[i].distance;
                ais.attribute=flagc;
                angle_index.push_back(ais);
                 
            }
        } 
    }
    flaga=1;
   while(flaga)     //剔除冗余点
    {
      size=angle_index.size();
      flaga=0;
      if(size>=3)
       {
        for( i=0;i<size-1;i++)
        {
            
            if((angle_index[i+1].index-angle_index[i].index)==1)
              {  angle_index.erase(angle_index.begin()+i);flaga=1; break;}
        }
       }
        size=angle_index.size();
       if(size>=3)
       {
        for( i=0;i<size-1;i++)
        {
            
            if((angle_index[size-i-1].index-angle_index[size-i].index)==1)
              {  angle_index.erase(angle_index.begin()+i+1);flaga=1; break;}
        }
       }
    }
}
double Scan_cloud::calculate_variance(std::vector<double> &db)
{
    double average=0.0;
//    for(int i=0;i<len;i++)average+=db[i]/(double)len;
    

}
